import spatialmath as sm

from ..robot import BipedRobot


class ManualPlanner:
    def __init__(self, robot: BipedRobot) -> None:
        self.robot = robot
        self.speed = 0.001

    def left_forward(self):
        T = self.robot.get_cartesian()
        left = (sm.SE3.Tx(self.speed) * T[0]).t
        right = T[1].t
        self.robot.set_cartesian(left, right)

    def left_backward(self):
        T = self.robot.get_cartesian()
        left = (sm.SE3.Tx(-self.speed) * T[0]).t
        right = T[1].t
        self.robot.set_cartesian(left, right)

    def left_left(self):
        T = self.robot.get_cartesian()
        left = (sm.SE3.Ty(self.speed) * T[0]).t
        right = T[1].t
        self.robot.set_cartesian(left, right)

    def left_right(self):
        T = self.robot.get_cartesian()
        left = (sm.SE3.Ty(-self.speed) * T[0]).t
        right = T[1].t
        self.robot.set_cartesian(left, right)

    def left_up(self):
        T = self.robot.get_cartesian()
        left = (sm.SE3.Tz(self.speed) * T[0]).t
        right = T[1].t
        self.robot.set_cartesian(left, right)

    def left_down(self):
        T = self.robot.get_cartesian()
        left = (sm.SE3.Tz(-self.speed) * T[0]).t
        right = T[1].t
        self.robot.set_cartesian(left, right)

    def right_forward(self):
        T = self.robot.get_cartesian()
        left = T[0].t
        right = (sm.SE3.Tx(self.speed) * T[1]).t
        self.robot.set_cartesian(left, right)

    def right_backward(self):
        T = self.robot.get_cartesian()
        left = T[0].t
        right = (sm.SE3.Tx(-self.speed) * T[1]).t
        self.robot.set_cartesian(left, right)

    def right_left(self):
        T = self.robot.get_cartesian()
        left = T[0].t
        right = (sm.SE3.Ty(self.speed) * T[1]).t
        self.robot.set_cartesian(left, right)

    def right_right(self):
        T = self.robot.get_cartesian()
        left = T[0].t
        right = (sm.SE3.Ty(-self.speed) * T[1]).t
        self.robot.set_cartesian(left, right)

    def right_up(self):
        T = self.robot.get_cartesian()
        left = T[0].t
        right = (sm.SE3.Tz(self.speed) * T[1]).t
        self.robot.set_cartesian(left, right)

    def right_down(self):
        T = self.robot.get_cartesian()
        left = T[0].t
        right = (sm.SE3.Tz(-self.speed) * T[1]).t
        self.robot.set_cartesian(left, right)
